#include "ScnsTof10120.c.h"

#if defined(SCNS_TOF10120_ENABLE)&&SCNS_TOF10120_ENABLE==1

//@scnsBsp:https://gitee.com/huoxingdawang/scnsDemos-MM32F3277_seekfree/tree/master/Libraries/scnsBsp/MM32F3277_seekfree/

#include "ScnsTimer.h"

static struct
{
    uint8 inited;
    uint16 maxD;
}rd[SCNS_TOF10120_MAX];

ScnsTof10120Result scnsTof10120Result[SCNS_TOF10120_MAX];

static ScnsTof10120Status WB(ScnsTof10120Enum tofN,uint8 addr,uint8 data)
{
    return scnsBspTof10120Write(tofN,&addr,1,&data,1);
}

static uint16 R2B(ScnsTof10120Enum tofN,uint8 addr)
{
    uint8 data[2];
    scnsBspTof10120Read(tofN,&addr,1,data,2);
    return (((uint16)data[0])<<8)|((uint16)data[1]);
}

ScnsTof10120Status scnsTof10120Init(ScnsTof10120Enum tofN)
{
    SCNS_ASSERT_ON_RUN(tofN<SCNS_TOF10120_MAX);
    scnsCheck(ScnsTof10120Status,scnsBspTof10120Init(tofN),SCNS_TOF10120_STATUS_OK);
    static const uint8 addr=0X0F;
    uint8 data;
    scnsCheckTimeOut(200*1000,scnsBspTof10120Read(tofN,&addr,1,&data,1)==SCNS_TOF10120_STATUS_OK,SCNS_TOF10120_STATUS_TIME_OUT);
    rd[tofN].maxD=R2B(tofN,0x0C);
    
    scnsCheck(ScnsTof10120Status,scnsBspTof10120PostInit(tofN),SCNS_TOF10120_STATUS_OK);
    rd[tofN].inited=1;
    return SCNS_TOF10120_STATUS_OK;
}

void scnsTof10120Interrupt(ScnsTof10120Enum tofN)
{
    SCNS_ASSERT_ON_RUN(tofN<SCNS_TOF10120_MAX);
    if(!rd[tofN].inited)
        return;
    
    uint16 d1=R2B(tofN,0x04);
    uint16 d2=R2B(tofN,0x04);
    
    if((d1==d2&&d1<=rd[tofN].maxD)||(d1<d2&&d2<=rd[tofN].maxD&&d2<=10+d1)||(d2<d1&&d1<=rd[tofN].maxD&&d1<=10+d2))
        scnsTof10120Result[tofN].distant=(d1+d2)>>1;
    
    scnsBspTof10120ResultUpdate(tofN);
}

#endif
